/*
 * Academic License - for use in teaching, academic research, and meeting
 * course requirements at degree granting institutions only.  Not for
 * government, commercial, or other organizational use.
 * File: arm_ik.c
 *
 * MATLAB Coder version            : 5.5
 * C/C++ source code generated on  : 14-Dec-2023 13:19:04
 */

/* Include Files */
#include "arm_ik.h"
#include <math.h>

/* Function Definitions */
/*
 * Arguments    : float X
 *                float Y
 *                float Z
 *                float *theta1
 *                float *theta2
 *                float *theta3
 *                float *theta4
 * Return Type  : void
 */
int arm_ik(float X, float Y, float Z, float *theta1, float *theta2, float *theta3, float *theta4)
{
	float Xt, Zt, Lt, Lp;
	float cosJ1, cosJ2, cosJ3, cosJ4, cosJ5;
	float J1, J2, J3, J4, J5;

	if ((X==0)&&(Y==0)&&(Z==0))
	{
		printf("input param error[1]. \r\n");
		return 0;
	}

	if (fabs(Z)>(L1+L2+L3+L4))
	{
//		if(Z>0)
//		{
//			Z = L1+L2+L3+L4;
//		}
//		else
//		{
//			Z = -(L1+L2+L3+L4);
//		}
		printf("input param error[3:%f].\r\n",Z);
		return 0;
	}

	if(fabs(Y)>(L2+L3+L4))
	{
//		if(Y)
//		{
//			Y = L2+L3+L4;
//		}
//		else
//		{
//			Y = -(L2+L3+L4);
//		}
		printf("input param error[3:%f].\r\n",Y);
		return 0;
	}

	if(fabs(X)>(L2+L3+L4))
	{
//		if(X)
//		{
//			X = L2+L3+L4;
//		}
//		else
//		{
//			X = -(L2+L3+L4);
//		}
		printf("input param error[4:%f].\r\n",X);
		return 0;
	}

    if (fabs(Y)<0.001f)
    {
//    	XDEBUG("[dbg]: 1\r\n");
        *theta1 = 0.0f;
        Xt = X-L4;
    }
    else
    {
//    	XDEBUG("[dbg]: 2\r\n");
    	*theta1 = atan2(Y,X);
    	if((fabs(*theta1)-RAD_90)<0.001f)
    	{
    		Lp = Y/sin(*theta1);
    	}
    	else
    	{
    		Lp = X/cos(*theta1);
    	}

        if (Lp>L4)
        {
 //       	XDEBUG("[dbg]: 2.1\r\n");
            Xt = Lp-L4;
        }
        else
        {
//        	XDEBUG("[dbg]: 2.2\r\n");
            Xt = L4;
        }
    }

    if (Z>=L1)
	{
//    	XDEBUG("[dbg]: 3\r\n");
        Zt = Z-L1;

		Lt = sqrt(Xt*Xt+Zt*Zt);

		cosJ1 = (L2*L2+Lt*Lt-L3*L3)/(2*L2*Lt);
		J1 = acos(cosJ1);
		cosJ2 = Xt/Lt;
		J2 = acos(cosJ2);
		cosJ5 = Zt/Lt;
		J5 = acos(cosJ5);

		if (fabs(J1-0.0f)<1e-6)
		{
//			XDEBUG("[dbg]: 3.1\r\n");
			*theta2 = RAD_90-J2;
			*theta3 = 0.0f;
			*theta4 = RAD_90-J5;
		}
		else
		{
//			XDEBUG("[dbg]: 3.2\r\n");
			cosJ3 = (L2*L2+L3*L3-Lt*Lt)/(2*L2*L3);
			J3 = acos(cosJ3);
			cosJ4 = (L3*L3+Lt*Lt-L2*L2)/(2*L3*Lt);
			J4 = acos(cosJ4);

			*theta2 = RAD_90-J1-J2;
			*theta3 = PI-J3;
			*theta4 = RAD_90-J4-J5;
		}
	}
    else
    {
//    	XDEBUG("[dbg]: 4\r\n");
        if ((Z>0.0f)&&(Z<L1))
        {
//        	XDEBUG("[dbg]: 4.1\r\n");
            Zt = L1-Z;
        }
        else
        {
//        	XDEBUG("[dbg]: 4.2\r\n");
            Zt = L1+fabs(Z);
        }
        Lt = sqrt(Xt*Xt+Zt*Zt);
        cosJ1 = (L2*L2+Lt*Lt-L3*L3)/(2*L2*Lt);
        J1 = acos(cosJ1);
        cosJ2 = Zt/Lt;
        J2 = acos(cosJ2);
        cosJ3 = (L2*L2+L3*L3-Lt*Lt)/(2*L2*L3);
        J3 = acos(cosJ3);
        cosJ4 = (L3*L3+Lt*Lt-L2*L2)/(2*L3*Lt);
        J4 = acos(cosJ4);
        cosJ5 = Xt/Lt;
        J5 = acos(cosJ5);

        *theta2 = PI-J1-J2;
        *theta3 = PI-J3;
        *theta4 = -(J5+J4);
    }
    //XDEBUG("[dbg]: ik-> Xt:%0.4f Zt:%0.4f Lt:%0.4f Lp:%0.4f \r\n",Xt, Zt, Lt, Lp);
    //XDEBUG("[dbg]: ik-> J1:%0.4f J2:%0.4f J3:%0.4f J4:%0.4f J5:%0.4f \r\n",rad2deg(J1),rad2deg(J2),rad2deg(J3),rad2deg(J4),rad2deg(J5));
    //XDEBUG("[dbg]: ik:[%0.2f %0.2f %0.2f %0.2f] \r\n", rad2deg(*theta1), rad2deg(*theta2), rad2deg(*theta3), rad2deg(*theta4));
    return 1;
}

/*
 * File trailer for arm_ik.c
 *
 * [EOF]
 */
